﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Drawing;
using Imaging;
using Microsoft.Kinect;
using KinectApplication;
using AForge.Math.Geometry;
using AForge;
using System.Runtime.InteropServices;


namespace KinectDwa.Parsers.Helpers
{
    //all the information you'll ever need about hand
    public class HandInfo
    {
        //note that width & height are sizes before resize
        public int Width;
        public int Height;

        //x,y within 640, 480 image
        public int X;
        public int Y;

        //x,y scaled to your screen size
        public int ScaledX;
        public int ScaledY;

        //remember those skele x,y for fun
        public int SkeletonX;
        public int SkeletonY;

        //Left corner
        public int Left;

        //top corner
        public int Top;

        //to show debug depth and color hand-part differently
        public short MinDepth;

        //hand array resized to 30x20
        public short[] ResizedArray;

        //hand array in its all glory
        public short[] HandArray;

        public short[] ErosionArray;

        //hand array in its all glory
        public short[] SkeleArray;

        public short[] RotatedHand;
        public int RotatedHandWidth;
        public int RotatedHandHeight;

        public HandInfo()
        {

        }

        public HandInfo(short[] array, short[] handArray, int width, int height, int x, int y, int scaledX, int scaledY, short minDepth, int left, int top, int skeleX, int skeleY)
        {
            ResizedArray = array;
            HandArray = handArray;
            Width = width;
            Height = height;
            X = x;
            Y = y;
            ScaledX = scaledX;
            ScaledY = scaledY;
            MinDepth = minDepth;
            Left = left;
            Top = top;
            SkeletonX = skeleX;
            SkeletonY = skeleY;
        }
    }

    public static class DepthDataHandler
    {
        private static QueueLinearFloodFiller floodFiller = new QueueLinearFloodFiller(640, 480, true, 300, 100);

        private static Queue<Point> previous = new Queue<Point>();

        private static int handWidth = 0;
        private static int handHeight = 0;

        public static HandInfo GetHandInfo(this short[] depthArray, int x, int y, int OriginalWidth, int OriginalHeight, double angle)
        {
            //x,y sanity checks
            x = Math.Min(x, 639);
            y = Math.Min(y, 479);

            x = Math.Max(x, 0);
            y = Math.Max(y, 0);

            angle = 90 - angle;

            floodFiller.FloodFill(depthArray, x, y);

            handWidth = floodFiller.Width2;
            handHeight = floodFiller.Height2;

            short[] handArray = Crop(floodFiller.Bits, floodFiller.Left, floodFiller.Top, handWidth, handHeight, floodFiller.MinDepth, floodFiller.MaxDepth);

            if (angle != 0 && MouseInputControl.gestureRotation == true)
            {
                var rotated = RotateBilinear(handArray, handWidth, handHeight, angle);

                if (rotated.Length == 0)
                {
                    return null;
                }
                //double the fun and shrink it back to oblivion
                handArray = Shrink(rotated, handWidth * 2, handHeight * 2);
            }

            //random min size sanity check
            if (handArray.Length > 30)
            {
                short[] resizedOriginalHandArray = ResizeBilinear(handArray, handWidth, handHeight, HandGestureReader.PicWidth, HandGestureReader.PicHeight);

                //return hand array
                var info = new HandInfo();

                //maintain original left, top, width and height here..
                info.Left = floodFiller.Left;
                info.Top = floodFiller.Top;
                info.Width = floodFiller.Width2;
                info.Height = floodFiller.Height2;

                //and rotated info
                info.RotatedHandWidth = handWidth;
                info.RotatedHandHeight = handHeight;


                info.HandArray = handArray;
                info.ResizedArray = resizedOriginalHandArray;

                return info;
            }
            else
            {
                return null;
            }
        }

        public static HandInfo GetHandInfoExperimental(this short[] depthArray, int x, int y, int OriginalWidth, int OriginalHeight, double angle)
        {
            //x,y sanity checks
            x = Math.Min(x, 639);
            y = Math.Min(y, 479);

            x = Math.Max(x, 0);
            y = Math.Max(y, 0);

            angle = 90 - angle;

            floodFiller.FloodFill(depthArray, x, y);

            handWidth = floodFiller.Width2;
            handHeight = floodFiller.Height2;

            int unrotatedHandWidth = floodFiller.Width2;
            int unrotatedHandHeight = floodFiller.Height2;

            short[] handArray = Crop(floodFiller.Bits, floodFiller.Left, floodFiller.Top, handWidth, handHeight, floodFiller.MinDepth, floodFiller.MaxDepth);

            //random min size
            if (handArray.Length > 30)
            {
                short[] resizedOriginalHandArray = ResizeBilinear(handArray, handWidth, handHeight, HandGestureReader.PicWidth, HandGestureReader.PicHeight);
                short[] originalHandArray = new short[handArray.Length];

                handArray.CopyTo(originalHandArray, 0);

                var rotated = RotateBilinear(handArray, handWidth, handHeight, angle);
                handArray = Shrink(rotated, handWidth * 2, handHeight * 2);

                if (handArray.Length < 30)
                {
                    return null;
                }

                Binarize(handArray);

                short[] skeleHandArray = new short[handArray.Length];
                short[] newArray = new short[handArray.Length];


                //start with skele x,y
                Point p = new Point(x, y);
                Point? point = null;

                if (Debugging.ErosionAndDilatation == true)
                {
                    DilatationBinary(handArray, newArray, handWidth, handHeight);

                    handArray = newArray;
                    newArray = new short[handArray.Length];

                    ErosionBinary(handArray, newArray, handWidth, handHeight);

                    handArray = newArray;
                    newArray = new short[handArray.Length];
                }
                //else if (Debugging.ErosionHandImage == true)
                //{
                //    for (int i = 0; i < 10; i++)
                //    {
                //        ErosionBinary(handArray, newArray, handWidth, handHeight);

                //        point = CalculateAveragePoint(newArray, handWidth, handHeight);

                //        if (point == null)
                //        {
                //            //remember last
                //            point = CalculateAveragePoint(handArray, handWidth, handHeight);

                //            break;
                //        }
                //        else
                //        {
                //            handArray = newArray;
                //            newArray = new short[handArray.Length];
                //        }
                //    }

                //    if (point.HasValue)
                //    {
                //        p = point.Value;

                //        //add the remaining..
                //        p = new Point(p.X + floodFiller.Left, p.Y + floodFiller.Top);

                //        previous.Enqueue(p);

                //        if (previous.Count > 3)
                //        {
                //            int averageX = p.X;
                //            int averageY = p.Y;

                //            foreach (var item in previous)
                //            {
                //                averageX += item.X;
                //                averageY += item.Y;
                //            }

                //            //latest x,y counts for two
                //            averageX = averageX / (previous.Count + 1);
                //            averageY = averageY / (previous.Count + 1);

                //            previous.Dequeue();

                //            p = new Point(averageX, averageY);
                //        }
                //    }
                //}

                if (Debugging.SkeletonizeHandImage == true)
                {
                    SimpleSkeletonization(handArray, skeleHandArray, (short)handWidth, (short)handHeight);
                }

                if (Debugging.FindQuadrilateralCorners)
                {
                    var pointcloud = DifferenceEdgeDetector(handArray, handWidth, handHeight);
                
                    pointcloud = FindGrahamConvexHull(pointcloud);

                    //pointcloud = GetShapeCorners(pointcloud, 165);

                    int lowerFifth = handHeight - (handHeight / 5);

                    pointcloud = ClosePointsMergingOptimizer(pointcloud);

                    for (int i = pointcloud.Count - 1; i >= 0; i--)
                    {
                        var item = pointcloud[i];

                        if (item.Y < lowerFifth)
                        {
                           // Debugging.ShowPointInHandImage(item.X, item.Y, System.Windows.Media.Colors.HotPink);
                        }
                        else
                        {
                            pointcloud.RemoveAt(i);
                        }
                    }


                    newArray = new short[skeleHandArray.Length];
                    DilatationBinary(skeleHandArray, newArray, handWidth, handHeight);
                    skeleHandArray = newArray;
                    newArray = new short[skeleHandArray.Length];
                    DilatationBinary(skeleHandArray, newArray, handWidth, handHeight);
                    skeleHandArray = newArray;
                    newArray = new short[skeleHandArray.Length];
                    ErosionBinary(skeleHandArray, newArray, handWidth, handHeight);
                    skeleHandArray = newArray;
                    newArray = new short[skeleHandArray.Length];


                    var skelePointcloud = DifferenceEdgeDetector(skeleHandArray, handWidth, handHeight);
                    skelePointcloud = FindGrahamConvexHull(skelePointcloud);
                    skelePointcloud = ClosePointsMergingOptimizer(skelePointcloud);
                    //skelePointcloud = GetShapeCorners(skelePointcloud, 160);


                    foreach (var item in skelePointcloud)
                    {

                        if (item.Y < lowerFifth)
                        {
                            Debugging.ShowPointInSkeleHandImage(item.X, item.Y, System.Windows.Media.Colors.HotPink);

                            foreach (var item2 in pointcloud)
                            {
                                if (item.DistanceTo(item2) < 5)
                                {
                                    int averageX = (int)((item.X + item2.X) / 2);
                                    int averageY = (int)((item.Y + item2.Y) / 2);

                                    //Debugging.ShowPointInHandImage(item.X, item.Y, System.Windows.Media.Color.FromArgb(255, 255, 0, 0));
                                    //Debugging.ShowPointInHandImage(item2.X, item2.Y, System.Windows.Media.Color.FromArgb(255, 0, 255, 255));
                                    Debugging.ShowPointInHandImage(averageX, averageY, System.Windows.Media.Color.FromArgb(255, 0, 255, 0));

                                }
                            }
                        }
                    }


                }

                int scaledX = (int)((System.Windows.Application.Current.MainWindow.ActualWidth * p.X) / OriginalWidth);
                int scaledY = (int)((System.Windows.Application.Current.MainWindow.ActualHeight * p.Y) / OriginalHeight);

                //returning  hand array
                var info = new HandInfo(resizedOriginalHandArray, originalHandArray, unrotatedHandWidth, unrotatedHandHeight, p.X, p.Y, scaledX, scaledY, floodFiller.MinDepth, floodFiller.Left, floodFiller.Top, x, y);

                info.ErosionArray = handArray;
                info.SkeleArray = skeleHandArray;

                info.RotatedHandWidth = handWidth;
                info.RotatedHandHeight = handHeight;

                return info;
            }
            else
            {
                return null;
            }
        }

        private static short[] Crop(short[] array, int X, int Y, int Width, int Height, short MinDepth, short MaxDepth)
        {
            short[] newArray = new short[Width * Height];

            for (var y = 0; y < Height; y++)
            {
                for (var x = 0; x < Width; x++)
                {
                    int index = ((y + Y) * 640) + x + X;

                    //we don't want those unknown values here
                    if (array[index] < -8)
                    {
                        //start from 255, the brightest!
                        //and remove mindepth
                        //short value = (short)(255 - (MinDepth - array[index]));



                        short divider = (short)(MaxDepth - MinDepth);
                        divider *= -1;

                        if (divider == 0)
                        {
                            // Whatever...
                            newArray[x + y * Width] = 100;
                        }
                        else
                        {
                            short depthFromMin = (short)(array[index] - MinDepth);
                            short depthPercent = (short)(depthFromMin * 99 / divider);
                            depthPercent *= -1;
                            ++depthPercent;

                            if (depthPercent < 1)
                            {
                                // TODO: breakpointti tänne, räjähtää kun Min (ja max?) depthi failaa, ei tykkää monesta floodfillistä samaan kuvaan?
                            }

                            if (depthPercent > 100)
                            {
                                // TODO: breakpointti tänne, räjähtää kun Min (ja max?) depthi failaa, ei tykkää monesta floodfillistä samaan kuvaan?
                            }

                            newArray[x + y * Width] = depthPercent;
                        }
                    }
                    else
                    {
                        newArray[x + y * Width] = 0;
                    }
                }
            }

            return newArray;
        }

        private static short[] ResizeBilinear(short[] sourceData, int width, int height, int newWidth, int newHeight)
        {
            short[] destinationData = new short[newWidth * newHeight];
            // get source image size

            int pixelSize = 1;
            int srcStride = width;
            int dstOffset = 0;
            double xFactor = (double)width / newWidth;
            double yFactor = (double)height / newHeight;

            // coordinates of source points
            double ox, oy, dx1, dy1, dx2, dy2;
            int ox1, oy1, ox2, oy2;
            // width and height decreased by 1
            int ymax = height - 1;
            int xmax = width - 1;
            // temporary pointers
            int tp1, tp2;
            int p1, p2, p3, p4;
            int dst = 0;

            // for each line
            for (int y = 0; y < newHeight; y++)
            {
                // Y coordinates
                oy = (double)y * yFactor;
                oy1 = (int)oy;
                oy2 = (oy1 == ymax) ? oy1 : oy1 + 1;
                dy1 = oy - (double)oy1;
                dy2 = 1.0 - dy1;

                // get temp pointers
                tp1 = oy1 * srcStride;
                tp2 = oy2 * srcStride;

                // for each pixel
                for (int x = 0; x < newWidth; x++)
                {
                    // X coordinates
                    ox = (double)x * xFactor;
                    ox1 = (int)ox;
                    ox2 = (ox1 == xmax) ? ox1 : ox1 + 1;
                    dx1 = ox - (double)ox1;
                    dx2 = 1.0 - dx1;

                    // get four points
                    p1 = sourceData[tp1 + ox1 * pixelSize];
                    p2 = sourceData[tp1 + ox2 * pixelSize];
                    p3 = sourceData[tp2 + ox1 * pixelSize];
                    p4 = sourceData[tp2 + ox2 * pixelSize];

                    // interpolate using 4 points
                    for (int i = 0; i < pixelSize; i++, p1++, dst++, p2++, p3++, p4++)
                    {
                        short hmm = (short)(dy2 * (dx2 * (p1) + dx1 * (p2)) + dy1 * (dx2 * (p3) + dx1 * (p4)));

                        destinationData[dst] = hmm;
                    }
                }
                dst += dstOffset;
            }

            return destinationData;
        }

        private static Point? CalculateAveragePoint(short[] depthArray, int width, int height)
        {
            int X = 0;
            int Y = 0;
            int Area = 0;

            for (int x = 0; x < width; x++)
            {
                for (int y = 0; y < height; y++)
                {
                    if (depthArray[y * width + x] != 0)
                    {
                        X += x;
                        Y += y;

                        Area++;
                    }
                }
            }

            if (Area > 0)
            {
                return new Point((int)(X / Area), (int)(Y / Area));
            }
            else
            {
                return null;
            }
        }

        //QueueLinearFloodFiller is now private class, deal with it   
        private class QueueLinearFloodFiller
        {

            private short toleranceMin = 0;
            private short toleranceMax = 0;
            // private short fillColor = 0;
            private bool fillDiagonally = false;
            private int Width = 0;
            private int Height = 0;
            public short[] Bits = null;
            private bool[] pixelsChecked;
            private short startColor;

            //how far should we continue the search..
            private int maxDistance = 50;
            //some magic numbers to find the optimal hand size, MagicNumber1 - (Sqrt(Depth) * MagicNumber2)
            double MagicNumber1 = 107.3d;
            double MagicNumber2 = 1.55d;


            private int startX;
            private int startY;

            private int left, right, top, bottom;

            //right is the new left.. don't ask
            public int Right
            {
                get
                {
                    return right;
                }
            }

            //left is the new right
            public int Left
            {
                get
                {
                    return left;
                }
            }

            public int Top
            {
                get
                {
                    return top;
                }
            }

            public int Width2
            {
                get
                {
                    return Right - Left;
                }
            }

            public int Height2
            {
                get
                {
                    return bottom - top;
                }
            }

            public short MinDepth = 0;
            public short MaxDepth = 0;

            FloodFillRangeQueue ranges = new FloodFillRangeQueue();

            public QueueLinearFloodFiller(int width, int height, bool diagonal, short toleranceMin, short toleranceMax)
            {

                //  array.CopyTo(Bits, 0);
                this.Width = width;
                this.Height = height;
                //  this.fillColor = fillColor;
                this.fillDiagonally = diagonal;
                this.toleranceMax = toleranceMax;
                this.toleranceMin = toleranceMin;


            }

            /// <summary>
            /// Fills the specified point on the bitmap with the currently selected fill color.
            /// </summary>
            /// <param name="pt">The starting point for the fill.</param>
            public void FloodFill(short[] array, int X, int Y)
            {
                Bits = array;

                this.pixelsChecked = new bool[array.Length];

                startX = X;
                startY = Y;

                left = X;
                right = X;
                top = Y;
                bottom = Y;

                ranges = new FloodFillRangeQueue(((Width + Height) / 2) * 5);//new Queue<FloodFillRange>();

                //***Get starting color.
                int x = X;
                int y = Y;

                int idx = CoordsToByteIndex(ref x, ref y);

                short realDepth = (short)(Bits[idx] >> DepthImageFrame.PlayerIndexBitmaskWidth);

                maxDistance = (int)(MagicNumber1 - (Math.Sqrt(realDepth) * MagicNumber2));

                startColor = realDepth;
                MinDepth = (short)-realDepth;
                MaxDepth = (short)-realDepth;

                bool[] pixelsChecked = this.pixelsChecked;

                //***Do first call to floodfill.
                LinearFill(ref x, ref y);

                //***Call floodfill routine while floodfill ranges still exist on the queue
                while (ranges.Count > 0)
                {
                    //**Get Next Range Off the Queue
                    FloodFillRange range = ranges.Dequeue();

                    //**Check Above and Below Each Pixel in the Floodfill Range
                    int downPxIdx = (Width * (range.Y + 1)) + range.StartX;//CoordsToPixelIndex(lFillLoc,y+1);
                    int upPxIdx = (Width * (range.Y - 1)) + range.StartX;//CoordsToPixelIndex(lFillLoc, y - 1);
                    int upY = range.Y - 1;//so we can pass the y coord by ref
                    int downY = range.Y + 1;
                    int tempIdx;
                    for (int i = range.StartX; i <= range.EndX; i++)
                    {
                        //*Start Fill Upwards
                        //if we're not above the top of the bitmap and the pixel above this one is within the color tolerance
                        tempIdx = CoordsToByteIndex(ref i, ref upY);
                        if (range.Y > 0 && (!pixelsChecked[upPxIdx]) && CheckPixel(ref tempIdx))
                            LinearFill(ref i, ref upY);

                        //*Start Fill Downwards
                        //if we're not below the bottom of the bitmap and the pixel below this one is within the color tolerance
                        tempIdx = CoordsToByteIndex(ref i, ref downY);
                        if (range.Y < (Height - 1) && (!pixelsChecked[downPxIdx]) && CheckPixel(ref tempIdx))
                            LinearFill(ref i, ref downY);
                        downPxIdx++;
                        upPxIdx++;
                    }

                }

                //    watch.Stop();
            }

            /// <summary>
            /// Finds the furthermost left and right boundaries of the fill area
            /// on a given y coordinate, starting from a given x coordinate, filling as it goes.
            /// Adds the resulting horizontal range to the queue of floodfill ranges,
            /// to be processed in the main loop.
            /// </summary>
            /// <param name="x">The x coordinate to start from.</param>
            /// <param name="y">The y coordinate to check at.</param>
            void LinearFill(ref int x, ref int y)
            {

                //cache some bitmap and fill info in local variables for a little extra speed
                short[] bitmapBits = this.Bits;
                bool[] pixelsChecked = this.pixelsChecked;
                //  short byteFillColor = this.fillColor;
                int bitmapWidth = this.Width;

                //***Find Left Edge of Color Area
                int lFillLoc = x; //the location to check/fill on the left
                int idx = CoordsToByteIndex(ref x, ref y); //the byte index of the current location
                int pxIdx = (bitmapWidth * y) + x;//CoordsToPixelIndex(x,y);
                while (true)
                {
                    if (pixelsChecked[pxIdx] == false)
                    {
                        //**fill with the color
                        //  bitmapBits[idx] = byteFillColor;
                        short value = (short)(Bits[idx] >> DepthImageFrame.PlayerIndexBitmaskWidth);
                        value *= -1;

                        if (MinDepth < value && value < -8)
                        {
                            MinDepth = value;
                        }
                        if (MaxDepth > value)
                        {
                            MaxDepth = value;
                        }

                        Bits[idx] = value;
                    }
                    //**indicate that this pixel has already been checked and filled
                    pixelsChecked[pxIdx] = true;

                    lFillLoc--;     //de-increment counter
                    pxIdx--;        //de-increment pixel index
                    idx--;// bitmapPixelFormatSize;//de-increment byte index

                    //**exit loop if we're at edge of bitmap or color area
                    if (lFillLoc <= 0 || (pixelsChecked[pxIdx]) || !CheckPixel(ref idx))
                        break;

                }
                lFillLoc++;

                //***Find Right Edge of Color Area
                int rFillLoc = x; //the location to check/fill on the left
                idx = CoordsToByteIndex(ref x, ref y);
                pxIdx = (bitmapWidth * y) + x;
                while (true)
                {
                    //**fill with the color
                    //  bitmapBits[idx] = byteFillColor;

                    if (pixelsChecked[pxIdx] == false)
                    {
                        short value = (short)(Bits[idx] >> DepthImageFrame.PlayerIndexBitmaskWidth);
                        value *= -1;

                        if (MinDepth < value && value < -8)
                        {
                            MinDepth = value;
                        }
                        if (MaxDepth > value)
                        {
                            MaxDepth = value;
                        }

                        Bits[idx] = value;
                    }

                    pixelsChecked[pxIdx] = true;

                    rFillLoc++;     //increment counter
                    pxIdx++;        //increment pixel index
                    idx++;// bitmapPixelFormatSize;//increment byte index

                    //**exit loop if we're at edge of bitmap or color area
                    if (rFillLoc >= bitmapWidth || pixelsChecked[pxIdx] || !CheckPixel(ref idx))
                        break;

                }
                rFillLoc--;

                //add range to queue
                FloodFillRange r = new FloodFillRange(lFillLoc, rFillLoc, y);
                ranges.Enqueue(ref r);
            }

            ///<summary>Sees if a pixel is within the color tolerance range.</summary>
            ///<param name="px">The byte index of the pixel to check, passed by reference to increase performance.</param>
            protected bool CheckPixel(ref int px)
            {
                short realDepth = (short)(Bits[px] >> DepthImageFrame.PlayerIndexBitmaskWidth);

                var value = (realDepth >= (startColor - toleranceMin)) && realDepth <= (startColor + toleranceMax);

                if (value == true)
                {
                    int X2 = px % Width;
                    int Y2 = (px - X2) / Width;

                    if (right < X2)
                    {
                        right = X2;
                    }

                    if (bottom < Y2)
                    {
                        bottom = Y2;
                    }

                    if (left > X2)
                    {
                        left = X2;
                    }

                    if (top > Y2)
                    {
                        top = Y2;
                    }

                    double a = (double)(X2 - startX);
                    double b = (double)(Y2 - startY);

                    //pixels too far from the center point, we don't want them either?
                    if (Math.Sqrt(a * a + b * b) > maxDistance)
                    {
                        return false;
                    }
                }

                return value;
            }

            ///<summary>Calculates and returns the byte index for the pixel (x,y).</summary>
            ///<param name="x">The x coordinate of the pixel whose byte index should be returned.</param>
            ///<param name="y">The y coordinate of the pixel whose byte index should be returned.</param>
            protected int CoordsToByteIndex(ref int x, ref int y)
            {
                return (Width * y) + x;
            }

            /// <summary>
            /// Returns the linear index for a pixel, given its x and y coordinates.
            /// </summary>
            /// <param name="x">The x coordinate of the pixel.</param>
            /// <param name="y">The y coordinate of the pixel.</param>
            /// <returns></returns>
            protected int CoordsToPixelIndex(int x, int y)
            {
                return (Width * y) + x;
            }

        }

        //make all 1-255 values to 1
        private unsafe static void Binarize(short[] array)
        {
            fixed (short* srcF = array)
            {
                short* src = srcF;

                for (int i = 0; i < array.Length; i++)
                {
                    if (*src > 0)
                    {
                        *src = 1;
                    }

                    src++;
                }
            }
        }

        // AForge Image Processing Library
        // AForge.NET framework
        // http://www.aforgenet.com/framework/
        //
        // Copyright © Andrew Kirillov, 2005-2009
        // andrew.kirillov@aforgenet.com
        private unsafe static void ErosionBinary(short[] sourceData, short[] destinationData, int width, int height)
        {

            // processing start and stop X,Y positions
            int startX = 1;
            int startY = 1;
            int stopX = width - 1;
            int stopY = height - 1;

            int dstStride = width;
            int srcStride = width;

            int dstOffset = 1;
            int srcOffset = 1;

            // image pointers
            fixed (short* srcF = sourceData)
            {
                short* src = srcF;

                fixed (short* dstF = destinationData)
                {
                    short* dst = dstF;

                    // align pointers by X and Y
                    src += (startX - 1) + (startY - 1) * srcStride;
                    dst += (startX - 1) + (startY - 1) * dstStride;

                    // --- process the first line setting all to black
                    for (int x = startX - 1; x < stopX; x++, src++, dst++)
                    {
                        *dst = 0;
                    }

                    *dst = 0;

                    src += srcOffset;
                    dst += dstOffset;

                    // --- process all lines except the last one
                    for (int y = startY; y < stopY; y++)
                    {
                        // set edge pixel to black
                        *dst = 0;

                        src++;
                        dst++;

                        // for each pixel
                        for (int x = startX; x < stopX; x++, src++, dst++)
                        {
                            *dst = (byte)(*src & src[-1] & src[1] &
                                src[-srcStride] & src[-srcStride - 1] & src[-srcStride + 1] &
                                src[srcStride] & src[srcStride - 1] & src[srcStride + 1]);
                        }

                        // set edge pixel to black
                        *dst = 0;

                        src += srcOffset;
                        dst += dstOffset;
                    }

                    // --- process the last line setting all to black

                    // for each pixel
                    for (int x = startX - 1; x < stopX; x++, src++, dst++)
                    {
                        *dst = 0;
                    }
                    *dst = 0;
                }
            }

        }

        private unsafe static void DilatationBinary(short[] sourceData, short[] destinationData, int width, int height)
        {


            int startX = 1;
            int startY = 1;
            int stopX = width - 1;
            int stopY = height - 1;

            int dstStride = width;
            int srcStride = width;

            int dstOffset = 1;
            int srcOffset = 1;

            // image pointers
            fixed (short* srcF = sourceData)
            {
                short* src = srcF;

                fixed (short* dstF = destinationData)
                {
                    short* dst = dstF;

                    // --- process the first line
                    *dst = (short)(*src | src[1] | src[srcStride] | src[srcStride + 1]);

                    src++;
                    dst++;

                    // for each pixel
                    for (int x = startX; x < stopX; x++, src++, dst++)
                    {
                        *dst = (short)(*src | src[-1] | src[1] | src[srcStride] | src[srcStride - 1] | src[srcStride + 1]);
                    }

                    *dst = (short)(*src | src[-1] | src[srcStride] | src[srcStride - 1]);

                    src += srcOffset;
                    dst += dstOffset;

                    // --- process all lines except the last one
                    for (int y = startY; y < stopY; y++)
                    {
                        *dst = (short)(*src | src[1] | src[-srcStride] | src[-srcStride + 1] | src[srcStride] | src[srcStride + 1]);

                        src++;
                        dst++;

                        // for each pixel
                        for (int x = startX; x < stopX; x++, src++, dst++)
                        {
                            *dst = (short)(*src | src[-1] | src[1] | src[-srcStride] | src[-srcStride - 1] | src[-srcStride + 1] | src[srcStride] | src[srcStride - 1] | src[srcStride + 1]);
                        }

                        *dst = (short)(*src | src[-1] | src[-srcStride] | src[-srcStride - 1] | src[srcStride] | src[srcStride - 1]);

                        src += srcOffset;
                        dst += dstOffset;
                    }

                    // --- process the last line
                    *dst = (short)(*src | src[1] | src[-srcStride] | src[-srcStride + 1]);

                    src++;
                    dst++;

                    // for each pixel
                    for (int x = startX; x < stopX; x++, src++, dst++)
                    {
                        *dst = (short)(*src | src[-1] | src[1] | src[-srcStride] | src[-srcStride - 1] | src[-srcStride + 1]);
                    }

                    *dst = (short)(*src | src[-1] | src[-srcStride] | src[-srcStride - 1]);
                }
            }
        }

        // AForge Image Processing Library
        // AForge.NET framework
        // http://www.aforgenet.com/framework/
        //
        // Copyright © Andrew Kirillov, 2005-2009
        // andrew.kirillov@aforgenet.com
        private unsafe static void SimpleSkeletonization(short[] sourceData, short[] destinationData, int width, int height)
        {
            // processing start and stop X,Y positions
            int startX = 0;
            int startY = 0;
            int stopX = width;
            int stopY = height;

            int dstStride = width;
            int srcStride = width;

            int srcOffset = 0;

            int start;
            short fg = 1;

            // image pointers
            fixed (short* srcF = sourceData)
            {
                short* src = srcF;
                short* src0 = srcF;

                fixed (short* dstF = destinationData)
                {
                    short* dst = dstF;
                    short* dst0 = dstF;

                    // horizontal pass

                    // allign pointers to the first pixel to process
                    src += (startY * srcStride + startX);
                    dst += (startY * dstStride);

                    // for each line
                    for (int y = startY; y < stopY; y++)
                    {

                        start = -1;
                        // for each pixel
                        for (int x = startX; x < stopX; x++, src++)
                        {
                            // looking for foreground pixel
                            if (start == -1)
                            {
                                if (*src == fg)
                                    start = x;
                                continue;
                            }

                            // looking for non foreground pixel
                            if (*src != fg)
                            {
                                dst[start + ((x - start) >> 1)] = fg;
                                start = -1;
                            }
                        }
                        if (start != -1)
                        {
                            dst[start + ((stopX - start) >> 1)] = fg;
                        }
                        src += srcOffset;
                        dst += dstStride;
                    }

                    // vertical pass

                    // align pointer to the first line to process
                    src0 += (startY * srcStride);

                    // for each column
                    for (int x = startX; x < stopX; x++)
                    {
                        src = src0 + x;
                        dst = dst0 + x;

                        start = -1;
                        // for each row
                        for (int y = startY; y < stopY; y++, src += srcStride)
                        {
                            // looking for foreground pixel
                            if (start == -1)
                            {
                                if (*src == fg)
                                    start = y;
                                continue;
                            }

                            // looking for non foreground pixel
                            if (*src != fg)
                            {
                                dst[dstStride * (start + ((y - start) >> 1))] = fg;
                                start = -1;
                            }
                        }
                        if (start != -1)
                        {
                            dst[dstStride * (start + ((stopY - start) >> 1))] = fg;
                        }
                    }
                }
            }
        }

        // AForge Image Processing Library
        // AForge.NET framework
        // http://www.aforgenet.com/framework/
        //
        // Copyright © Andrew Kirillov, 2007-2009
        // andrew.kirillov@aforgenet.com
        private unsafe static List<IntPoint> FindEdgePoints(short[] sourceData, int width, int height)
        {
            // collection of points
            List<IntPoint> points = new List<IntPoint>();

            // collect edge points
            fixed (short* srcF = sourceData)
            {
                short* src = srcF;

                bool lineIsEmpty;

                // for each row
                for (int y = 0; y < height; y++)
                {
                    lineIsEmpty = true;

                    // scan from left to right
                    for (int x = 0; x < width; x++)
                    {
                        if (src[x] != 0)
                        {
                            points.Add(new IntPoint(x, y));
                            lineIsEmpty = false;
                            break;
                        }
                    }

                    if (!lineIsEmpty)
                    {
                        // scan from right to left
                        for (int x = width - 1; x >= 0; x--)
                        {
                            if (src[x] != 0)
                            {
                                points.Add(new IntPoint(x, y));
                                break;
                            }
                        }
                    }

                    src += width;
                }
            }

            return points;

            //old tech
            //if (points.Count > 0)
            //{
            //    return PointsCloud.FindQuadrilateralCorners(points);
            //}
            //else
            //{
            //    return points;
            //}
        }


        private unsafe static List<IntPoint> DifferenceEdgeDetector(short[] sourceData, int width, int height)
        {
            List<IntPoint> Points = new List<IntPoint>();

            // processing start and stop X,Y positions
            int startX = 1;
            int startY = 1;
            int stopX = startX + width - 2;
            int stopY = startY + height - 2;

            int dstStride = width;
            int srcStride = width;

            int dstOffset = dstStride - width + 2;
            int srcOffset = srcStride - width + 2;

            int d, max;

            // data pointers
            fixed (short* srcF = sourceData)
            {
                short* src = srcF;

                // allign pointers
                src += srcStride * startY + startX;

                // for each line
                for (int y = startY; y < stopY; y++)
                {
                    // for each pixel
                    for (int x = startX; x < stopX; x++, src++)
                    {
                        // left diagonal
                        max = (int)src[-srcStride - 1] - src[srcStride + 1];
                        if (max < 0)
                            max = -max;

                        // right diagonal
                        d = (int)src[-srcStride + 1] - src[srcStride - 1];
                        if (d < 0)
                            d = -d;
                        if (d > max)
                            max = d;
                        // vertical
                        d = (int)src[-srcStride] - src[srcStride];
                        if (d < 0)
                            d = -d;
                        if (d > max)
                            max = d;
                        // horizontal
                        d = (int)src[-1] - src[1];
                        if (d < 0)
                            d = -d;
                        if (d > max)
                            max = d;

                        if (max > 0)
                        {
                            Points.Add(new IntPoint(x, y));
                        }
                    }
                    src += srcOffset;
                }

            }

            return Points;
        }


        //returns points that are not too steep.. not that useful for us
        private unsafe static List<IntPoint> GetShapeCorners(List<IntPoint> shape, float maxAngleToKeep)
        {

            // optimized shape
            List<IntPoint> optimizedShape = new List<IntPoint>();

            if (shape.Count <= 3)
            {
                // do nothing if shape has 3 points or less
                optimizedShape.AddRange(shape);
            }
            else
            {
                float angle = 0;

                // add first 2 points to the new shape
                optimizedShape.Add(shape[0]);
                optimizedShape.Add(shape[1]);
                int pointsInOptimizedHull = 2;

                for (int i = 2, n = shape.Count; i < n; i++)
                {
                    // add new point
                    optimizedShape.Add(shape[i]);
                    pointsInOptimizedHull++;

                    // get angle between 2 vectors, which start from the next to last point
                    angle = GetAngleBetweenVectors(optimizedShape[pointsInOptimizedHull - 2],
                        optimizedShape[pointsInOptimizedHull - 3], optimizedShape[pointsInOptimizedHull - 1]);

                    if ((angle > maxAngleToKeep) &&
                         ((pointsInOptimizedHull > 3) || (i < n - 1)))
                    {
                        // remove the next to last point
                        optimizedShape.RemoveAt(pointsInOptimizedHull - 2);
                        pointsInOptimizedHull--;
                    }
                }

                if (pointsInOptimizedHull > 3)
                {
                    // check the last point
                    angle = GetAngleBetweenVectors(optimizedShape[pointsInOptimizedHull - 1],
                        optimizedShape[pointsInOptimizedHull - 2], optimizedShape[0]);

                    if (angle > maxAngleToKeep)
                    {
                        optimizedShape.RemoveAt(pointsInOptimizedHull - 1);
                        pointsInOptimizedHull--;
                    }

                    if (pointsInOptimizedHull > 3)
                    {
                        // check the first point
                        angle = GetAngleBetweenVectors(optimizedShape[0],
                            optimizedShape[pointsInOptimizedHull - 1], optimizedShape[1]);

                        if (angle > maxAngleToKeep)
                        {
                            optimizedShape.RemoveAt(0);
                        }
                    }
                }
            }

            return optimizedShape;
        }

        private unsafe static List<IntPoint> FindGrahamConvexHull(List<IntPoint> points)
        {
            List<PointToProcess> pointsToProcess = new List<PointToProcess>();

            // convert input points to points we can process
            foreach (IntPoint point in points)
            {
                pointsToProcess.Add(new PointToProcess(point));
            }

            // find a point, with lowest X and lowest Y
            int firstCornerIndex = 0;
            PointToProcess firstCorner = pointsToProcess[0];

            for (int i = 1, n = pointsToProcess.Count; i < n; i++)
            {
                if ((pointsToProcess[i].X < firstCorner.X) ||
                     ((pointsToProcess[i].X == firstCorner.X) && (pointsToProcess[i].Y < firstCorner.Y)))
                {
                    firstCorner = pointsToProcess[i];
                    firstCornerIndex = i;
                }
            }

            // remove the just found point
            pointsToProcess.RemoveAt(firstCornerIndex);

            // find K (tangent of line's angle) and distance to the first corner
            for (int i = 0, n = pointsToProcess.Count; i < n; i++)
            {
                int dx = pointsToProcess[i].X - firstCorner.X;
                int dy = pointsToProcess[i].Y - firstCorner.Y;

                // don't need square root, since it is not important in our case
                pointsToProcess[i].Distance = dx * dx + dy * dy;
                // tangent of lines angle
                pointsToProcess[i].K = (dx == 0) ? float.PositiveInfinity : (float)dy / dx;
            }

            // sort points by angle and distance
            pointsToProcess.Sort();

            List<PointToProcess> convexHullTemp = new List<PointToProcess>();

            // add first corner, which is always on the hull
            convexHullTemp.Add(firstCorner);
            // add another point, which forms a line with lowest slope
            convexHullTemp.Add(pointsToProcess[0]);
            pointsToProcess.RemoveAt(0);

            PointToProcess lastPoint = convexHullTemp[1];
            PointToProcess prevPoint = convexHullTemp[0];

            while (pointsToProcess.Count != 0)
            {
                PointToProcess newPoint = pointsToProcess[0];

                // skip any point, which has the same slope as the last one or
                // has 0 distance to the first point
                if ((newPoint.K == lastPoint.K) || (newPoint.Distance == 0))
                {
                    pointsToProcess.RemoveAt(0);
                    continue;
                }

                // check if current point is on the left side from two last points
                if ((newPoint.X - prevPoint.X) * (lastPoint.Y - newPoint.Y) - (lastPoint.X - newPoint.X) * (newPoint.Y - prevPoint.Y) < 0)
                {
                    // add the point to the hull
                    convexHullTemp.Add(newPoint);
                    // and remove it from the list of points to process
                    pointsToProcess.RemoveAt(0);

                    prevPoint = lastPoint;
                    lastPoint = newPoint;
                }
                else
                {
                    // remove the last point from the hull
                    convexHullTemp.RemoveAt(convexHullTemp.Count - 1);

                    lastPoint = prevPoint;
                    prevPoint = convexHullTemp[convexHullTemp.Count - 2];
                }
            }

            // convert points back
            List<IntPoint> convexHull = new List<IntPoint>();

            foreach (PointToProcess pt in convexHullTemp)
            {
                convexHull.Add(pt.ToPoint());
            }

            return convexHull;
        }

        private class PointToProcess : IComparable
        {
            public int X;
            public int Y;
            public float K;
            public float Distance;

            public PointToProcess(IntPoint point)
            {
                X = point.X;
                Y = point.Y;

                K = 0;
                Distance = 0;
            }

            public int CompareTo(object obj)
            {
                PointToProcess another = (PointToProcess)obj;

                return (K < another.K) ? -1 : (K > another.K) ? 1 :
                    ((Distance > another.Distance) ? -1 : (Distance < another.Distance) ? 1 : 0);
            }

            public IntPoint ToPoint()
            {
                return new IntPoint(X, Y);
            }
        }

        private static List<IntPoint> ClosePointsMergingOptimizer(List<IntPoint> shape)
        {
            float maxDistanceToMerge = 10;

            // optimized shape
            List<IntPoint> optimizedShape = new List<IntPoint>();

            if (shape.Count <= 3)
            {
                // do nothing if shape has 3 points or less
                optimizedShape.AddRange(shape);
            }
            else
            {
                float distance = 0;

                shape[0].Good = true;

                // add first point to the new shape
                optimizedShape.Add(shape[0]);
                int pointsInOptimizedHull = 1;

                for (int i = 1, n = shape.Count; i < n; i++)
                {
                    distance = optimizedShape[pointsInOptimizedHull - 1].DistanceTo(shape[i]);

                    if ((distance <= maxDistanceToMerge) && (pointsInOptimizedHull + (n - i) > 3))
                    {
                        // merge
                        optimizedShape[pointsInOptimizedHull - 1] = (optimizedShape[pointsInOptimizedHull - 1] + shape[i]) / 2;
                        optimizedShape[pointsInOptimizedHull - 1].Good = true;
                    }
                    else
                    {
                        optimizedShape.Add(shape[i]);
                        pointsInOptimizedHull++;
                    }
                }

                if (pointsInOptimizedHull > 3)
                {
                    // check the last and first points
                    distance = optimizedShape[pointsInOptimizedHull - 1].DistanceTo(optimizedShape[0]);

                    if (distance <= maxDistanceToMerge)
                    {
                        // merge
                        optimizedShape[0] = (optimizedShape[pointsInOptimizedHull - 1] + optimizedShape[0]) / 2;
                        optimizedShape[0].Good = true;
                        optimizedShape.RemoveAt(pointsInOptimizedHull - 1);
                    }
                }
            }

            return optimizedShape;
        }

        private static float GetAngleBetweenVectors(Point startPoint, Point vector1end, Point vector2end)
        {
            float x1 = vector1end.X - startPoint.X;
            float y1 = vector1end.Y - startPoint.Y;

            float x2 = vector2end.X - startPoint.X;
            float y2 = vector2end.Y - startPoint.Y;

            return (float)(Math.Acos((x1 * x2 + y1 * y2) / (Math.Sqrt(x1 * x1 + y1 * y1) * Math.Sqrt(x2 * x2 + y2 * y2))) * 180.0 / Math.PI);
        }

        // AForge Image Processing Library
        // AForge.NET framework
        // http://www.aforgenet.com/framework/
        //
        // Copyright © Andrew Kirillov, 2005-2009
        // andrew.kirillov@aforgenet.com
        private unsafe static List<IntPoint> MoravecCornersDetector(short[] sourceData, int width, int height)
        {
            int[] xDelta = new int[8] { -1, 0, 1, 1, 1, 0, -1, -1 };
            int[] yDelta = new int[8] { -1, -1, -1, 0, 1, 1, 1, 0 };

            int stride = width;
            int pixelSize = 1;

            int windowSize = 3;
            // threshold which is used to filter interest points
            int threshold = 3;

            // window radius
            int windowRadius = windowSize / 2;

            // offset
            int offset = stride - windowSize * pixelSize;

            // create moravec cornerness map
            int[,] moravecMap = new int[height, width];

            fixed (short* srcF = sourceData)
            {
                short* ptr = srcF;
                // byte* ptr = (byte*)image.ImageData.ToPointer();

                // for each row
                for (int y = windowRadius, maxY = height - windowRadius; y < maxY; y++)
                {
                    // for each pixel
                    for (int x = windowRadius, maxX = width - windowRadius; x < maxX; x++)
                    {
                        int minSum = int.MaxValue;

                        // go through 8 possible shifting directions
                        for (int k = 0; k < 8; k++)
                        {
                            // calculate center of shifted window
                            int sy = y + yDelta[k];
                            int sx = x + xDelta[k];

                            // check if shifted window is within the image
                            if (
                                (sy < windowRadius) || (sy >= maxY) ||
                                (sx < windowRadius) || (sx >= maxX)
                            )
                            {
                                // skip this shifted window
                                continue;
                            }

                            int sum = 0;

                            short* ptr1 = ptr + (y - windowRadius) * stride + (x - windowRadius) * pixelSize;
                            short* ptr2 = ptr + (sy - windowRadius) * stride + (sx - windowRadius) * pixelSize;

                            // for each windows' rows
                            for (int i = 0; i < windowSize; i++)
                            {
                                // for each windows' pixels
                                for (int j = 0, maxJ = windowSize * pixelSize; j < maxJ; j++, ptr1++, ptr2++)
                                {
                                    int dif = *ptr1 - *ptr2;
                                    sum += dif * dif;
                                }
                                ptr1 += offset;
                                ptr2 += offset;
                            }

                            // check if the sum is mimimal
                            if (sum < minSum)
                            {
                                minSum = sum;
                            }
                        }

                        // threshold the minimum sum
                        if (minSum < threshold)
                        {
                            minSum = 0;
                        }

                        moravecMap[y, x] = minSum;
                    }
                }
            }

            // collect interesting points - only those points, which are local maximums
            List<IntPoint> cornersList = new List<IntPoint>();

            // for each row
            for (int y = windowRadius, maxY = height - windowRadius; y < maxY; y++)
            {
                // for each pixel
                for (int x = windowRadius, maxX = width - windowRadius; x < maxX; x++)
                {
                    int currentValue = moravecMap[y, x];

                    // for each windows' rows
                    for (int i = -windowRadius; (currentValue != 0) && (i <= windowRadius); i++)
                    {
                        // for each windows' pixels
                        for (int j = -windowRadius; j <= windowRadius; j++)
                        {
                            if (moravecMap[y + i, x + j] > currentValue)
                            {
                                currentValue = 0;
                                break;
                            }
                        }
                    }

                    // check if this point is really interesting
                    if (currentValue != 0)
                    {
                        cornersList.Add(new IntPoint(x, y));
                    }
                }
            }

            return cornersList;
        }

        private unsafe static short[] Shrink(short[] sourceData,int width, int height)
        {
            var size = CalculateNewImageSize(sourceData, width, height);

            var destinationData = new short[size.Width * size.Height];

            // get destination image size
            int newWidth = size.Width;
            int newHeight = size.Height;

            handWidth = newWidth;
            handHeight = newHeight;

            int srcStride = width;
            int dstStride = size.Width;
            int copySize = newWidth;

            // do the job
            fixed (short* srcF = sourceData)
            {
                short* src = srcF;

                fixed (short* srcD = destinationData)
                {
                    short* dst = srcD;

                    src += (minY * srcStride);
                    src += minX;
                   
                    // copy image
                    for (int y = 0; y < newHeight; y++)
                    {
                        CopyUnmanagedMemory(dst, src, copySize);
                        dst += dstStride;
                        src += srcStride;
                    }
                }
            }

            return destinationData;
        }

        public unsafe static short* CopyUnmanagedMemory(short* dst, short* src, int count)
        {
            //TODO and warning, count * 2 because of short..
            return memcpy(dst, src, count * 2);
        }

        // Win32 memory copy function
        [DllImport("ntdll.dll", CallingConvention = CallingConvention.Cdecl)]
        private static unsafe extern short* memcpy(
            short* dst,
            short* src,
            int count);

        static int minX;
        static int minY;

        private unsafe static Size CalculateNewImageSize(short[] sourceData, int width, int height)
        {
            int offset = 0;

            minX = width;
            minY = height;
            int maxX = 0;
            int maxY = 0;

            fixed (short* srcF = sourceData)
            {
                short* src = srcF;

                // grayscale
                for (int y = 0; y < height; y++)
                {
                    for (int x = 0; x < width; x++, src++)
                    {
                        if (*src != 0)
                        {
                            if (x < minX)
                                minX = x;
                            if (x > maxX)
                                maxX = x;
                            if (y < minY)
                                minY = y;
                            if (y > maxY)
                                maxY = y;
                        }
                    }
                    src += offset;
                }

            }

            // check
            if ((minX == width) && (minY == height) && (maxX == 0) && (maxY == 0))
            {
                minX = minY = 0;
            }

            return new Size(maxX - minX + 1, maxY - minY + 1);
        }

        private unsafe static short[] RotateBilinear(short[] sourceData, int width, int height, double angle)
        {
            double oldXradius = (double)(width - 1) / 2;
            double oldYradius = (double)(height - 1) / 2;

            // get destination image size
            int newWidth = width * 2;
            int newHeight = height * 2;

            var destinationData = new short[newWidth * newHeight];

            double newXradius = (double)(newWidth - 1) / 2;
            double newYradius = (double)(newHeight - 1) / 2;

            // angle's sine and cosine
            double angleRad = -angle * Math.PI / 180;
            double angleCos = Math.Cos(angleRad);
            double angleSin = Math.Sin(angleRad);

            int srcStride = width;
            int dstOffset = 0;

            fixed (short* srcF = sourceData)
            {
                short* src = srcF;

                fixed (short* srcD = destinationData)
                {
                    short* dst = srcD;
                    // do the job
                    //  byte* src = (byte*)sourceData.ImageData.ToPointer();
                    //   byte* dst = (byte*)destinationData.ImageData.ToPointer();

                    // destination pixel's coordinate relative to image center
                    double cx, cy;
                    // coordinates of source points
                    double ox, oy, tx, ty, dx1, dy1, dx2, dy2;
                    int ox1, oy1, ox2, oy2;
                    // width and height decreased by 1
                    int ymax = height - 1;
                    int xmax = width - 1;
                    // temporary pointers
                    short* p1, p2, p3, p4;


                    // grayscale
                    cy = -newYradius;
                    for (int y = 0; y < newHeight; y++)
                    {
                        // do some pre-calculations of source points' coordinates
                        // (calculate the part which depends on y-loop, but does not
                        // depend on x-loop)
                        tx = angleSin * cy + oldXradius;
                        ty = angleCos * cy + oldYradius;

                        cx = -newXradius;
                        for (int x = 0; x < newWidth; x++, dst++)
                        {
                            // coordinates of source point
                            ox = tx + angleCos * cx;
                            oy = ty - angleSin * cx;

                            // top-left coordinate
                            ox1 = (int)ox;
                            oy1 = (int)oy;

                            // validate source pixel's coordinates
                            if ((ox1 < 0) || (oy1 < 0) || (ox1 >= width) || (oy1 >= height))
                            {
                                // fill destination image with filler
                                *dst = 0;
                            }
                            else
                            {
                                // bottom-right coordinate
                                ox2 = (ox1 == xmax) ? ox1 : ox1 + 1;
                                oy2 = (oy1 == ymax) ? oy1 : oy1 + 1;

                                if ((dx1 = ox - (double)ox1) < 0)
                                    dx1 = 0;
                                dx2 = 1.0 - dx1;

                                if ((dy1 = oy - (double)oy1) < 0)
                                    dy1 = 0;
                                dy2 = 1.0 - dy1;

                                p1 = src + oy1 * width;
                                p2 = src + oy2 * width;

                                // interpolate using 4 points
                                *dst = (short)(
                                    dy2 * (dx2 * p1[ox1] + dx1 * p1[ox2]) +
                                    dy1 * (dx2 * p2[ox1] + dx1 * p2[ox2]));
                            }
                            cx++;
                        }
                        cy++;
                        dst += dstOffset;
                    }

                }
            }

            return destinationData;
        }
    }
}
